﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010, 2011
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2RopeJoint.h
// Revision   : r141
// Change-Date: 2010-10-10
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/b2RopeJoint.cpp
// Revision   : r141
// Change-Date: 2010-10-10
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Rope joint definition. This requires two body anchor points and
    /// a maximum lengths. 
    /// Note: by default the connected objects will not collide. 
    /// see collideConnected in b2JointDef. 
    /// </summary>
    public class RopeJointDef : JointDef
    {
	    public RopeJointDef()
	    {
		    type = JointType.ROPE;
            localAnchorA = new Vector2(-1.0f, 0.0f);
            localAnchorB = new Vector2(1.0f, 0.0f);
            maxLength = 0.0f;
        }

        /// <summary>The local anchor point relative to bodyA's origin.</summary>
	    public Vector2 localAnchorA;

        /// <summary>The local anchor point relative to bodyB's origin.</summary>
	    public Vector2 localAnchorB;

        /// <summary>
        /// The maximum length of the rope.
        /// Warning: this must be larger than b2_linearSlop or
        /// the joint will have no effect.
        /// </summary>
	    public float maxLength;
    };

    /// <summary>
    /// A rope joint enforces a maximum distance between two points 
    /// on two bodies. It has no other effect.
    /// Warning: if you attempt to change the maximum length during 
    /// the simulation you will get some non-physical behavior.
    /// A model that would allow you to dynamically modify the length 
    /// would have some sponginess, so I chose not to implement it
    /// that way. See DistanceJoint if you want to dynamically
    /// control length.
    /// </summary>
    public class RopeJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchorA);
        }

	    public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchorB);
        }

	    public override Vector2 GetReactionForce(float inv_dt)
        {
            return (inv_dt * m_impulse) * m_u;
        }

	    public override float GetReactionTorque(float inv_dt)
        {
            //B2_NOT_USED(inv_dt);
            return 0.0f;
        }

        public float GetMaxLength()
        {
            return m_maxLength;
        }

        public LimitState GetLimitState()
        {
            return m_state;
        }

        // Limit:
 	    // C = norm(pB - pA) - L
 	    // u = (pB - pA) / norm(pB - pA)
 	    // Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
 	    // J = [-u -cross(rA, u) u cross(rB, u)]
 	    // K = J * invM * JT
 	    //   = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2

	    public RopeJoint(RopeJointDef def)
            : base(def)
        {
            m_localAnchorA = def.localAnchorA;
            m_localAnchorB = def.localAnchorB;

            m_maxLength = def.maxLength;

            m_mass = 0.0f;
            m_impulse = 0.0f;
            m_state = LimitState.INACTIVE;
            m_length = 0.0f;
        }

	    internal override void InitVelocityConstraints(TimeStep step)
        {
	        Body bA = m_body1;
 	        Body bB = m_body2;

            m_rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter);
 	        m_rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter);

            // Rope axis
 	        m_u = bB.m_sweep.c + m_rB - bA.m_sweep.c - m_rA;

            m_length = m_u.Length();

            float C = m_length - m_maxLength;
 	        if (C > 0.0f)
 	        {
 	            m_state = LimitState.AT_UPPER;
 	        }
 	        else
 	        {
 	            m_state = LimitState.INACTIVE;
 	        }
 	
            if (m_length > PhysicSettings.LinearSlop)
 	        {
 	            m_u *= 1.0f / m_length;
 	        }
 	        else
 	        {
 	            m_u = Vector2.Zero;
 	            m_mass = 0.0f;
 	            m_impulse = 0.0f;
 	            return;
 	        }

            // Compute effective mass.
 	        float crA = Vector2Extensions.Cross(m_rA, m_u);
 	        float crB = Vector2Extensions.Cross(m_rB, m_u);
 	        float invMass = bA.m_invMass + bA.m_invI * crA * crA + bB.m_invMass + bB.m_invI * crB * crB;

            m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (step.warmStarting)
 	        {
 	            // Scale the impulse to support a variable time step.
 	            m_impulse *= step.dtRatio;

                Vector2 P = m_impulse * m_u;
 	            bA.m_linearVelocity -= bA.m_invMass * P;
 	            bA.m_angularVelocity -= bA.m_invI * Vector2Extensions.Cross(m_rA, P);
 	            bB.m_linearVelocity += bB.m_invMass * P;
 	            bB.m_angularVelocity += bB.m_invI * Vector2Extensions.Cross(m_rB, P);
 	        }
 	        else
 	        {
 	            m_impulse = 0.0f;
 	        }
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
	        Body bA = m_body1;
 	        Body bB = m_body2;

            // Cdot = dot(u, v + cross(w, r))
 	        Vector2 vA = bA.m_linearVelocity + Vector2Extensions.Cross(bA.m_angularVelocity, m_rA);
 	        Vector2 vB = bB.m_linearVelocity + Vector2Extensions.Cross(bB.m_angularVelocity, m_rB);
 	        float C = m_length - m_maxLength;
 	        float Cdot = Vector2.Dot(m_u, vB - vA);

            // Predictive constraint.
 	        if (C < 0.0f)
 	        {
 	            Cdot += step.inv_dt * C;
 	        }

            float impulse = -m_mass * Cdot;
 	        float oldImpulse = m_impulse;
 	        m_impulse = MathHelper.Min(0.0f, m_impulse + impulse);
 	        impulse = m_impulse - oldImpulse;

            Vector2 P = impulse * m_u;
 	        bA.m_linearVelocity -= bA.m_invMass * P;
 	        bA.m_angularVelocity -= bA.m_invI * Vector2Extensions.Cross(m_rA, P);
 	        bB.m_linearVelocity += bB.m_invMass * P;
 	        bB.m_angularVelocity += bB.m_invI * Vector2Extensions.Cross(m_rB, P);
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
	        //B2_NOT_USED(baumgarte);

	        Body bA = m_body1;
 	        Body bB = m_body2;

            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter);
 	        Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter);

            Vector2 u = bB.m_sweep.c + rB - bA.m_sweep.c - rA;

            u.Normalize();
            float length = u.Length(); 
 	        float C = length - m_maxLength;

            C = MathHelper.Clamp(C, 0.0f, PhysicSettings.MaxLinearCorrection);

            float impulse = -m_mass * C;
 	        Vector2 P = impulse * u;

            bA.m_sweep.c -= bA.m_invMass * P;
 	        bA.m_sweep.a -= bA.m_invI * Vector2Extensions.Cross(rA, P);
 	        bB.m_sweep.c += bB.m_invMass * P;
 	        bB.m_sweep.a += bB.m_invI * Vector2Extensions.Cross(rB, P);

            bA.SynchronizeTransform();
 	        bB.SynchronizeTransform();

            return length - m_maxLength < PhysicSettings.LinearSlop;
        }

        internal Vector2 m_localAnchorA;
        internal Vector2 m_localAnchorB;

        internal float m_maxLength;
        internal float m_length;

        // Jacobian info
        internal Vector2 m_u, m_rA, m_rB;

        // Effective mass
        internal float m_mass;

        // Impulses for accumulation/warm starting.
        internal float m_impulse;

        internal LimitState m_state;
    }
}
